#include "Rais_DXMotor.h"

DXMotor::DXMotor(byte speedPin, byte dirPin){
	DXMotor::setFrequency();
	
	this->dirPin = dirPin;
	this->speedPin = speedPin;
	
	pinMode(dirPin, OUTPUT);
	pinMode(speedPin, OUTPUT);
	digitalWrite(dirPin, LOW);
	analogWrite(speedPin, 0);
	
	speed = 0;
}

void DXMotor::setFrequency(){
	static bool wasSet = false;
	if(wasSet) return;
	wasSet = true;
	//the frequency is 970Hz
	//this part of codes are come from DuXingTech. - MeDCMotor.h  
	#if defined(__AVR_ATmega32U4__) //for ATmega32U4
	TCCR1A = _BV(WGM10);
	TCCR1B = _BV(CS11) | _BV(CS10) | _BV(WGM12);

	TCCR3A = _BV(WGM30);
	TCCR3B = _BV(CS31) | _BV(CS30) | _BV(WGM32);

	TCCR4B = _BV(CS42) | _BV(CS41) | _BV(CS40);
	TCCR4D = 0;

	#elif defined(__AVR_ATmega328__) //for ATmega328	//UNO is this

	TCCR1A = _BV(WGM10);
	TCCR1B = _BV(CS11) | _BV(CS10) | _BV(WGM12);

	TCCR2A = _BV(WGM21) | _BV(WGM20);
	TCCR2B = _BV(CS22);

	#elif defined(__AVR_ATmega2560__) //for ATmega2560
	TCCR1A = _BV(WGM10);
	TCCR1B = _BV(CS11) | _BV(CS10) | _BV(WGM12);

	TCCR2A = _BV(WGM21) | _BV(WGM20);
	TCCR2B = _BV(CS22);
	#endif
}

DXMotor& DXMotor::getM1(){
	static DXMotor m1(6, 7);
	return m1;
}

DXMotor& DXMotor::getM2(){
	static DXMotor m2(5, 4);
	return m2;
}

void DXMotor::run(int dir_speed){
	if(speed ==  dir_speed) return;
	
	dir_speed = (dir_speed > 255) ? 255 : dir_speed;
	dir_speed = (dir_speed < -255) ? -255 : dir_speed;
	if(dir_speed >= 0){
		//正转
		digitalWrite(dirPin, LOW);
		delayMicroseconds(5);
		analogWrite(speedPin, dir_speed);
	}else {
		//反转
		digitalWrite(dirPin, HIGH);
		delayMicroseconds(5);
		analogWrite(speedPin, 255 - dir_speed);
	}
	speed = dir_speed;
}

int DXMotor::getSpeed(){
	return speed;
}

DXMotor& M1 = DXMotor::getM1();
DXMotor& M2 = DXMotor::getM2();
